#include "MatrixSystem.hpp"

using namespace Data;

MatrixStack::MatrixStack() {
  _mat_mode = MatrixMode::MODELVIEW_MATRIX;
  _modelView_mat_stack.push(glm::identity<glm::mat4>());
  _custom_mat_stack.push(glm::identity<glm::mat4>());
  _perspective_mat_stack.push(glm::identity<glm::mat4>());
}

MatrixStack &MatrixStack::getMatrixStack() {
  static MatrixStack stack;
  return stack;
}

void MatrixStack::pushMatrix() {
  switch (_mat_mode) {
  case MatrixMode::MODELVIEW_MATRIX:
    _modelView_mat_stack.push(_modelView_mat_stack.top());
    break;
  case MatrixMode::CUSTOM_MATRIX:
    _custom_mat_stack.push(_custom_mat_stack.top());
    break;
  case MatrixMode::PERSPECTIVE_MATRIX:
    _perspective_mat_stack.push(_perspective_mat_stack.top());
    break;
  default:
    break;
  }
}

void MatrixStack::popMatrix() {
  switch (_mat_mode) {
  case MatrixMode::MODELVIEW_MATRIX:
    _modelView_mat_stack.pop();
    break;
  case MatrixMode::CUSTOM_MATRIX:
    _custom_mat_stack.pop();
    break;
  case MatrixMode::PERSPECTIVE_MATRIX:
    _perspective_mat_stack.pop();
    break;
  default:
    break;
  }
}

void MatrixStack::translate(const glm::vec3 &position) {
  if (_mat_mode != MatrixMode::MODELVIEW_MATRIX)
    _mat_mode = MatrixMode::MODELVIEW_MATRIX;
  _modelView_mat_stack.top() = glm::translate(_modelView_mat_stack.top(), position);
}

void MatrixStack::rotate(const glm::vec3 &axis, float d) {
  if (_mat_mode != MatrixMode::MODELVIEW_MATRIX)
    setMatrixMode(MatrixMode::MODELVIEW_MATRIX);
  _modelView_mat_stack.top() = glm::rotate(_modelView_mat_stack.top(), d, axis);
}

void MatrixStack::scale(const glm::vec3 &s) {
  if (_mat_mode != MatrixMode::MODELVIEW_MATRIX)
    setMatrixMode(MatrixMode::MODELVIEW_MATRIX);
  _modelView_mat_stack.top() = glm::scale(_modelView_mat_stack.top(), s);
}

void MatrixStack::shear(const glm::mat4 &matrix) {
  // TODO: Shear a matrix
}

void MatrixStack::setCurrentIdentity() { return setIdentityByMode(_mat_mode); }

void MatrixStack::setIdentityByMode(MatrixMode mode) {
  switch (mode) {
  case MatrixMode::MODELVIEW_MATRIX:
    _modelView_mat_stack.top() = glm::identity<glm::mat4>();
    break;
  case MatrixMode::CUSTOM_MATRIX:
    _custom_mat_stack.top() = glm::identity<glm::mat4>();
    break;
  case MatrixMode::PERSPECTIVE_MATRIX:
    _perspective_mat_stack.top() = glm::identity<glm::mat4>();
    break;
  default:
    break;
  }
}

void MatrixStack::setCurrentMatrix(const glm::mat4 &m) {
  setTopMatrixByMode(m, _mat_mode);
}

void MatrixStack::setTopMatrixByMode(const glm::mat4 &m, MatrixMode mode) {
  switch (mode) {
  case MatrixMode::MODELVIEW_MATRIX:
    _modelView_mat_stack.top() = m;
    break;
  case MatrixMode::CUSTOM_MATRIX:
    _custom_mat_stack.top() = m;
    break;
  case MatrixMode::PERSPECTIVE_MATRIX:
    _perspective_mat_stack.top() = m;
    break;
  default:
    break;
  }
}

glm::mat4 MatrixStack::getCurrentMatrix() { return getCurrentMatrixByMode(_mat_mode); }

// TODO: Can not get any benifit by this way, may be use map
glm::mat4 MatrixStack::getCurrentMatrixByMode(MatrixMode mode) {
  switch (mode) {
  case MatrixMode::MODELVIEW_MATRIX:
    return _modelView_mat_stack.top();
    break;
  case MatrixMode::CUSTOM_MATRIX:
    return _custom_mat_stack.top();
    break;
  case MatrixMode::PERSPECTIVE_MATRIX:
    return _perspective_mat_stack.top();
    break;
  default:
    return glm::identity<glm::mat4>();
    break;
  }
}

void MatrixStack::multiMatrix(const glm::mat4 &matrix) {
  switch (_mat_mode) {
  case MatrixMode::MODELVIEW_MATRIX:
    _modelView_mat_stack.top() = matrix * _modelView_mat_stack.top();
    break;
  case MatrixMode::CUSTOM_MATRIX:
    _custom_mat_stack.top() = matrix * _custom_mat_stack.top();
    break;
  case MatrixMode::PERSPECTIVE_MATRIX:
    _perspective_mat_stack.top() = matrix * _perspective_mat_stack.top();
    break;
  default:
    break;
  }
}

glm::mat4 MatrixStack::getMVP() { return _perspective_mat_stack.top() * getMV(); }

glm::mat4 MatrixStack::getMV() { return _modelView_mat_stack.top(); }